use cmsis_rtos2_rs::os_delay;
use periph_utils::GpioConfig;
use periph_utils::stm32f1;
use crate::app::cargo::CargoErr;

use super::sensor;


const TIME_DURATION: u32 = 8 * 1000;

#[derive(GpioConfig)]
#[gpio(init = "gpio_init",
    io(name = "pump", io = "PC8", mode = "OUT_PP", set, reset),
    io(name = "pump_v1", io = "PC9", mode = "OUT_PP", set, reset),
    io(name = "pump_v2", io = "PC10", mode = "OUT_PP", set, reset),
    io(name = "air_pump", io = "PC11", mode = "OUT_PP", set, reset),
    io(name = "air_v", io = "PC12", mode = "OUT_PP", set, reset),
    io(name = "led", io = "PC13", mode = "OUT_PP", set, reset),
)]
struct Pin;


pub unsafe fn init() {
    Pin::gpio_init();
}

pub fn air_open() {
    Pin::air_pump_set();
    Pin::air_v_reset();
}

pub fn air_drop() {
    Pin::air_pump_reset();
    Pin::air_v_set();
}

pub fn air_close() {
    Pin::air_pump_reset();
    Pin::air_v_reset();
}

pub fn cy_close() {
    Pin::pump_reset();
    Pin::pump_v1_reset();
    Pin::pump_v2_reset();
}

pub fn cy_run_pick() -> Result<(), CargoErr> {

    let mut time_count = 0;

    Pin::pump_set();
    Pin::pump_v1_set();
    air_open();
    loop {

        if sensor::is_cy_bottom_checked() {
            break;
        } 

        if sensor::is_compr_checked() {
            break;
        }

        time_count += 20;
        os_delay(20);
        if time_count >= TIME_DURATION {
            return Err(CargoErr::CyBottomFail);
        }
    }

    time_count = 0;
    loop {
        if sensor::is_compr_checked() {
            break;
        }
        time_count += 20;
        os_delay(20);
        if time_count >= 3000 {
            return Err(CargoErr::PickFail);
        }
    }

    cy_to_top()?;

    Ok(())
}

pub fn cy_to_top() -> Result<(), CargoErr> {

    let mut time_count = 0;
    let mut ret = Ok(());

    Pin::pump_set();
    Pin::pump_v1_reset();

    loop {
        if sensor::is_cy_top_checked() {
            break;
        }
        time_count += 20;
        os_delay(20);
        if time_count >= TIME_DURATION {
            ret = Err(CargoErr::CyTopFail);
            break;
        }
    }

    ret
}

pub fn cy_to_bottom() -> Result<(), CargoErr> {

    let mut ret = Ok(());
    let mut time_count = 0;
    Pin::pump_set();
    Pin::pump_v1_set();

    loop {
        if sensor::is_cy_bottom_checked() {
            break;
        }
        time_count += 20;
        os_delay(20);
        if time_count >= TIME_DURATION {
            ret = Err(CargoErr::CyBottomFail);
            break;
        }
    }
    ret
}

pub fn cy_to_inner() -> Result<(), CargoErr> {

    if sensor::is_cy_inner_checked() {
        return Ok(());
    }

    let mut ret = Ok(());
    let mut time_count = 0;
    Pin::pump_set();
    Pin::pump_v2_set();

    loop {
        if sensor::is_cy_inner_checked() {
            break;
        }
        time_count += 20;
        os_delay(20);
        if time_count >= TIME_DURATION {
            ret = Err(CargoErr::CyInnerFail);
            break;
        }
    }
    ret
}

pub fn cy_to_extern() -> Result<(), CargoErr> {

    if sensor::is_cy_extern_checked() {
        return Ok(());
    }

    let mut ret= Ok(());
    let mut time_count = 0;
    Pin::pump_set();
    Pin::pump_v2_reset();

    loop {
        if sensor::is_cy_extern_checked() {
            break;
        }
        time_count += 20;
        os_delay(20);
        if time_count >= TIME_DURATION {
            ret = Err(CargoErr::CyExternFail);
            break;
        }
    }
    ret
}

pub fn led_open() {
    Pin::led_set();
}

pub fn led_close() {
    Pin::led_reset();
}

